#pragma once

#include "VisioCommon.h"
#include <opencv2\opencv.hpp>
#include "Camera.h"
#include "Meshes.h"
#include "ImageMatch.h"
#include "Triangulator.h"
#include "ImageDataBase.h"
#include "MeshInstances.h"
#include <vector>
#include <set>

struct SPoint;
struct Compare3D
{
	bool operator()(cv::Point3d a, cv::Point3d b) const;
};

typedef std::pair<cv::Point3d, SPoint> PointCloudEntryPair;
typedef std::map<cv::Point3d, SPoint, Compare3D> PointCloudData;
typedef std::pair<CCamera*,UINT> tKeyData;
typedef std::vector<tKeyData> tKeyVector;
typedef std::set<int> OutliersSet;


struct SPoint
{
	cv::Point3d point;
	KeyColor color;
	tKeyVector keys;
	CTrackData* pTrack;
};


class CPointCloud
{
public:
	void CreatePoints(const CImageMatch* pMatch, const CamerasVector& cams);
	void AddPoint(SPoint& point);
	void MergeKeys(SPoint& dst, SPoint& src);
	int Size() const;
	UINT GetViewMask(UINT NumCameras, char* vmask, double* projections, cv::Point3d* points, int NumProjections, int* remap) const;
	void UpdatePoints(double* points, UINT NumPoints);
	void UpdatePoints( Point3d* points, UINT NumPoints, int* remap );
	bool empty() const;
	Point3d Get3DPoint(UINT index) const;
	void AddKeyData(UINT index, CCamera* pCam, UINT keyPointIndex);
	void AddPoints(CamerasVector& cams, CImageDataBase& ImageDataBase);
	bool IsWellConditioned(CCamera** ppCams, Point2d* projs, UINT NumPoints);
	double ComputeRayAngle(Point2d p, Point2d q, CCamera* CamA, CCamera* CamB);
	int GetNumberOfProjections() const;
	void VerifyViewsWithBundler();
	void CheckViews();
	void SortKeyData(tKeyVector& keys);
	void AddKeys(SPoint& point, tKeyData KeyData);
	void AddKeys(SPoint& point, tKeyData KeyData, UINT PointIndex);
	void RemoveOutliers( OutliersSet outliers );
	CPointCloudMeshInstance* CreateMeshInstance();
	void PrintToFile(FILE* pFile) const;
private:
	CTriangulator m_Trinangulator;
	PointCloudData m_PointCloud;
	CPointCloudMesh* m_pMesh;
	std::vector<SPoint> _points;
};